// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceHardwareGrip.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware
{
    using System;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Base class providing grip and grip pose information for hardware specific
    /// combination of an arm and gripper
    /// </summary>
    public abstract class PicknPlaceHardwareGrip : IPicknPlaceHardwareGrip
    {
        /// <summary>
        /// Return the tool control point pose for a grip
        /// </summary>
        /// <param name="grip">The grip</param>
        /// <param name="surface">Surface offset</param>
        /// <param name="score">Output score measure of how far the pose is to limits. Bigger is better</param>
        /// <returns>Tool control point pose</returns>
        /// <exception cref="ArgumentException">Hardware cannot execute the grip</exception>
        public abstract Pose ArmPoseFromGrip(GripState grip, double surface, out double score);

        /// <summary>
        /// Pre grip plane from a grip plane
        /// </summary>
        /// <param name="gripPlane">Input grip plane</param>
        /// <returns>Pre grip plane</returns>
        public abstract Vector4 PreGripPlane(Vector4 gripPlane);

        /// <summary>
        /// Retrieve the pre grip from a grip
        /// </summary>
        /// <param name="grip">The grip</param>
        /// <returns>Pre grip center</returns>
        public abstract Vector3 PreGripCenter(GripState grip);

        /// <summary>
        /// Return the amount that gripper should open expressed as a percent of the maximum open configuration in "open" state
        /// </summary>
        /// <param name="gripDimension">Dimension of the object at the grip center orthogonal to grip axis and parallel to the palm plane</param>
        /// <returns>Amount of grip opening expressed as a percent</returns>
        public abstract double GetPercentOpenForOpenState(double gripDimension);

        /// <summary>
        /// Return the amount that gripper should open expressed as a percent of the maximum open configuration in "close" state
        /// </summary>
        /// <param name="gripDimension">Dimension of the object at the grip center orthogonal to grip axis and parallel to the palm plane</param>
        /// <returns>Amount of grip opening expressed as a percent</returns>
        public abstract double GetPercentOpenForCloseState(double gripDimension);

        /// <summary>
        /// Return a parallel  plane but offset by an amount
        /// </summary>
        /// <param name="plane">Input plane</param>
        /// <param name="planeOffset">plane offset</param>
        /// <returns>The offset plane</returns>
        protected Vector4 OffsetPalmPlane(Vector4 plane, double planeOffset)
        {
            Vector4 offsetPlane = new Vector4(plane);
            offsetPlane.W = offsetPlane.W + planeOffset;
            return offsetPlane;
        }
    }
}
